/*
 * motor.c
 *
 *  Created on: Dec 8, 2019
 *      Author: Standa
 */

#include "motor.h"
#include "utility.h"
#include "tim.h"
#include "main.h"


#define MO_SPEED_MIN	100
#define MO_SPEED_MAX	999


void mo_Init(void){

}


void mo_SetSpeed(int16_t speed){

	LL_TIM_DisableCounter(TIM3);
	LL_TIM_SetCounter(TIM3, 0);
	LL_TIM_CC_DisableChannel(TIM3, LL_TIM_CHANNEL_CH1);
	LL_TIM_CC_DisableChannel(TIM3, LL_TIM_CHANNEL_CH2);
	LL_GPIO_ResetOutputPin(PA_GPIO_Port, PA_Pin);
	LL_GPIO_ResetOutputPin(PB_GPIO_Port, PB_Pin);


	if(speed > 0){
		UT_SATURATE(speed, MO_SPEED_MIN, MO_SPEED_MAX);
		//LL_GPIO_ResetOutputPin(PA_GPIO_Port, PA_Pin);
		//LL_GPIO_SetOutputPin(PB_GPIO_Port, PB_Pin);

		LL_TIM_OC_SetCompareCH1(TIM3, speed);
		LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH1);
		LL_TIM_EnableCounter(TIM3);
	}
	else if(speed < 0){
		speed *= -1;
		UT_SATURATE(speed, MO_SPEED_MIN, MO_SPEED_MAX);
		//LL_GPIO_SetOutputPin(PA_GPIO_Port, PA_Pin);
		//LL_GPIO_ResetOutputPin(PB_GPIO_Port, PB_Pin);

		LL_TIM_OC_SetCompareCH2(TIM3, speed);
		LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH2);
		LL_TIM_EnableCounter(TIM3);
	}
	else{



	}



}
